import lejos.nxt.Button;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.DifferentialPilot;


public class LeftRight {
	static int RotateSpeed=60;
	static int Speed=30;
	static UltrasonicSensor sonic=new UltrasonicSensor(SensorPort.S1);
	static DifferentialPilot pilot = new DifferentialPilot(2.2f, 4.7f, Motor.B, Motor.A, true);  // parameters in inches
	static int newDistance;
	static int leftDistance;
	static int rightDistance;
	static int treshold=22;
	static int rotangle = 45;

	public static void main(String[] args) {
		// TODO Auto-generated method stub

		pilot.setRotateSpeed(Speed);  // cm per second
		while(!Button.ENTER.isPressed())
		{
			int value = sonic.getDistance();
			//System.out.println(value);
			if(value>treshold)
			{
				pilot.forward();
				//pilot.travel(value);
			}
			else
			{
				//turn left = positive
				rotate(rotangle);
				leftDistance = sonic.getDistance();
				rotate(-rotangle*2);
				rightDistance = sonic.getDistance();

				if( leftDistance > rightDistance ) {
					rotate(2*rotangle+90-rotangle);
				} else {
					rotate(-(90-rotangle));
				}
				while(pilot.isMoving())
					Thread.yield();
			}				
		}		

	}

	public static void rotate(int angle){
		//left is positive, right negative
		pilot.setRotateSpeed(RotateSpeed);
		pilot.rotate(angle);
		pilot.setRotateSpeed(RotateSpeed);	
	}

	public static void rotateLeft()
	{
		pilot.setRotateSpeed(RotateSpeed);
		pilot.rotate(90);        // degree clockwise
		pilot.setRotateSpeed(Speed);
	}
	public static void rotateRight()
	{
		pilot.setRotateSpeed(RotateSpeed);
		pilot.rotate(-90);
		pilot.setRotateSpeed(Speed);

	}

}
